/*
 * fixed_point.h
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   fixed_point.h
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   16 nov 2011
 * @brief  Fixed point number representation and basic math routines. The fixed
 *         point numbers are unsigned and occupy 32 bits, of which 16 for the
 *         whole part and 16 for the fractional part.
 */

#ifndef FIXED_POINT_H
#define FIXED_POINT_H

#include <stdint.h>

/**
 * The number of fractional bits in the fixed point representation.
 */
#define NB_FRACTIONAL_BITS     16

/**
 * 2 ^ (\a NB_FRACTIONAL_BITS)
 */
#define FRACTIONAL_MULTIPLIER  65536.0f

/**
 * The largest representable fixed point 16u16 number.
 */
#define FXP_16U16_MAX       UINT32_MAX

/**
 * The number 0 represented as a fixed point 16u16.
 */
#define FXP_16U16_ZERO      0

/**
 * The number 1 represented as a fixed point 16u16.
 */
#define FXP_16U16_ONE       ((fxp16u16_t)1 << NB_FRACTIONAL_BITS)

/**
 * The number 0.5 represented as a fixed point 16u16.
 */
#define FXP_16U16_ONE_HALF  (FXP_16U16_ONE >> 1)

/**
 * Fixed point 16u16 are internally represented as 32-bit unsigned integers.
 */
typedef uint32_t fxp16u16_t;


/**
 * Converts a fixed point 16u16 number into an unsigned 16-bit integer, by
 * rounding to the nearest integer.
 *
 * @param x  the fixed point number to convert
 * @return The integer closest to the number represented by \a x, in the form
 *         of a 16-bit unsigned integer. If the fractional part of \a x is
 *         equal to 0.5, it is rounded up.
 */
inline uint16_t
fxp16u16_to_uint16(fxp16u16_t x)
{
  return (x + FXP_16U16_ONE_HALF) >> NB_FRACTIONAL_BITS;
}

/**
 * Converts a fixed point 16u16 number into a float.
 *
 * @param x  the fixed point number to convert
 * @return The float closest to the number represented by \a x.
 */
inline float
fxp16u16_to_float(fxp16u16_t x)
{
  return ((float)x / FRACTIONAL_MULTIPLIER);
}

/**
 * Converts an unsigned 16-bit integer into a fixed point 16u16 number.
 *
 * @param x  the 16-bit integer to convert
 * @return The fixed point 16u16 number of which the whole part is equal to \a
 *         x and of which the fractional part is 0.
 */
inline fxp16u16_t
fxp16u16_from_uint16(uint16_t x)
{
  return ((fxp16u16_t)x) << NB_FRACTIONAL_BITS;
}

/**
 * Converts a float into a fixed point 16u16 number.
 *
 * @param x  the float to convert
 * @return The fixed point 16u16 number closest to the given float, if the
 *         float is within the representable range of the fixed point 16u16
 *         datatype (0 to (65536 - eps)). The result is undefined otherwise.
 */
inline fxp16u16_t
fxp16u16_from_float(float x)
{
  return (fxp16u16_t)((x * FRACTIONAL_MULTIPLIER) + 0.5f);
}


/**
 * Converts a float into a fixed point 16u16 number, using saturated arithmetic.
 *
 * @param x  the float to convert
 * @return The fixed point 16u16 number closest to the given float, if the
 *         float is within the representable range of the fixed point 16u16
 *         datatype (0 to (65536 - eps)). If the float is less than 0, the 
 *         fixed point 16u16 representation of 0 is returned. If the float is
 *         larger than the maximum representable fixed point 16u16 value, \a 
 *         FXP_16U16_MAX is returned.
 */
inline fxp16u16_t
fxp16u16_from_float_s(float x)
{
  if (x >= fxp16u16_to_float(FXP_16U16_MAX)) {
    return FXP_16U16_MAX;
  } else if (x < 0) {
    return FXP_16U16_ZERO;
  } else {
    return fxp16u16_from_float(x);
  }
}


/**
 * Returns the product of two fixed point 16u16 numbers, if that product is
 * within the range of representable fixed point 16u16 numbers.
 *
 * @param x  the first factor of the product
 * @param y  the second factor of the product
 * @return (x * y) if that is within the range of representable fixed point
 *         16u16 numbers, or an undefined value otherwise.
 */
inline fxp16u16_t
fxp16u16_mul(fxp16u16_t x, fxp16u16_t y)
{
  uint64_t result = ((uint64_t)x * (uint64_t)y);
  result += FXP_16U16_ONE_HALF;
  return (fxp16u16_t)(result >> NB_FRACTIONAL_BITS);
}

/**
 * Returns the quotient of two fixed point 16u16 numbers.
 *
 * @param x  the dividend
 * @param y  the divisor
 * @return  x / y
 */
inline fxp16u16_t
fxp16u16_div(fxp16u16_t x, fxp16u16_t y)
{
  uint64_t temp = (uint64_t)x << NB_FRACTIONAL_BITS;
  temp += (y >> 1); // For correct rounding
  return (fxp16u16_t)(temp / y);
}

/**
 * Returns the quotient of a fixed point 16u16 number divided by an unsigned
 * 16-bit integer.
 *
 * @param x  the dividend
 * @param y  the divisor
 * @return  x / y
 */
inline fxp16u16_t
fxp16u16_div_uint16(fxp16u16_t x, uint16_t y)
{
  uint64_t temp = (uint64_t)x;
  temp += (y >> 1); // For correct rounding
  return (fxp16u16_t)(temp / y);
}

/**
 * Returns the square of a fixed point 16u16 number.
 *
 * @param x  the number of which to return the square
 * @return  x * x
 */
inline fxp16u16_t
fxp16u16_square(fxp16u16_t x)
{
  return fxp16u16_mul(x,x);
}

#endif //FIXED_POINT_H
